/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.camera;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.util.Point2D;
import com.team1160.breakaway.util.Point2DDouble;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera.ResolutionT;
import edu.wpi.first.wpilibj.camera.AxisCamera.WhiteBalanceT;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.EllipseDescriptor;
import edu.wpi.first.wpilibj.image.EllipseMatch;
import edu.wpi.first.wpilibj.image.MonoImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import java.util.Hashtable;
import java.util.Timer;
import java.util.TimerTask;
import java.util.Vector;

/**
 *
 * @author CJ
 */
public class Targetting {
//TODO Mango

    public static final double AREA_RATIO = .75;

    public static final int CENTER_DELTA = 20;

    protected EllipseDescriptor ellipseDescriptor = new EllipseDescriptor(3, 200, 3, 100);

    protected EllipseMatch[] targets;
    protected Point2D center;
    protected Point2DDouble normalizedCenter;
    protected AxisCamera camera;
    protected final Object LOCK = new Object();
    protected Hashtable properties;
    //protected int delta;
    protected boolean center_detected;

    //protected final Integer DEFAULT_DELTA = new Integer(10);
    protected final ResolutionT DEFAULT_RESOL = AxisCamera.ResolutionT.k640x480;
    protected final WhiteBalanceT DEFUALT_WHIBA = AxisCamera.WhiteBalanceT.automatic;
    protected final Integer DEFAULT_BRIGHT = new Integer(75);

    protected final String PROP_BRIGHT = "property.bright";
    //protected final String PROP_DELTA = "property.delta";
    protected final String PROP_RESOL = "property.resol";
    protected final String PROP_WHIBA = "property.whiba";


    protected int cameraWidth  = 0;
    protected int cameraHalfWidth  = 0;
    protected int cameraHeight = 0;
    protected int cameraHalfHeight = 0;

    protected EllipseCenterPartitioner centerPartitioner;
    protected EllipseAreaPartitioner areaChecker;
    protected Timer cameraTimer;
    protected TimerTask cameraTask;

    protected long delayTime = 200;

    protected boolean running = false;

    //---------------------------------------------------------------------

    /**
     * Default constructor with no property overrides.
     */

    public Targetting()
    {
        this.properties = null;
        initialize();
    }

    //---------------------------------------------------------------------

    /**
     * Constructor with overrides hashtable
     * @param properties Hashtable of property name to property value,
     * as a string.  Can be null.
     */

    public Targetting(Hashtable properties)
    {
        this.properties = properties;
        initialize();
    }

    //---------------------------------------------------------------------

    protected void initialize()
    {
        this.center_detected = false;
        this.center          = null;

        //create utility fields
        this.centerPartitioner = new EllipseCenterPartitioner();
        this.areaChecker = new EllipseAreaPartitioner();

        //init camera
        this.initCamera();

        //init properties
        this.initFromProperties();

        //init timer
        this.initTimer();
    }

    //---------------------------------------------------------------------

    protected void initCamera()
    {
        this.camera = AxisCamera.getInstance();

        this.cameraWidth  = this.camera.getResolution().width;
        this.cameraHeight = this.camera.getResolution().height;

        this.cameraHalfWidth  = (this.cameraWidth - 1 ) / 2;
        this.cameraHalfHeight = (this.cameraHeight - 1) / 2;
    }

    //---------------------------------------------------------------------

    protected void initFromProperties()
    {

        if(this.properties != null){
            try{
                scanProperties(properties);
            }catch(Exception e){
                e.printStackTrace();
            }
        }
        else
        {
            defaultProperties();
        }
        setProperties();
    }

    //---------------------------------------------------------------------

    protected void defaultProperties()
    {
        this.properties = new Hashtable();
        //this.properties.put(this.PROP_DELTA,
        //        this.DEFAULT_DELTA);
        this.properties.put(this.PROP_RESOL,
                this.DEFAULT_RESOL);
        this.properties.put(this.PROP_WHIBA,
                this.DEFUALT_WHIBA);
        this.properties.put(this.PROP_BRIGHT,
                this.DEFAULT_BRIGHT);
    }

    //---------------------------------------------------------------------

    protected void scanProperties(Hashtable properties) throws IllegalArgumentException
    {
//        if(!properties.contains(this.PROP_DELTA) ||
//                !properties.contains(this.DEFAULT_RESOL) ||
//                !properties.contains(this.DEFUALT_WHIBA) ||
//                !properties.contains(this.DEFAULT_BRIGHT))
        if(
                !properties.contains(this.DEFAULT_RESOL) ||
                !properties.contains(this.DEFUALT_WHIBA) ||
                !properties.contains(this.DEFAULT_BRIGHT))
            throw new IllegalArgumentException("Cannot scan a property value");
        this.properties = properties;
    }

    //---------------------------------------------------------------------

    protected void setProperties()
    {
        this.camera.writeWhiteBalance((WhiteBalanceT)(this.properties.get(this.PROP_WHIBA)));
        this.camera.writeResolution((ResolutionT)(this.properties.get(this.PROP_RESOL)));
        this.camera.writeBrightness(((Integer)(this.properties.get(this.PROP_BRIGHT))).intValue());
    }

    //---------------------------------------------------------------------

    protected void initTimer()
    {
        //construct the timer
        this.cameraTimer = new Timer();

        //specify the timer task action

        //schedulethe task
    }

    public void start()
    {
        if(this.cameraTimer == null)
            throw new IllegalStateException("Cannot start, stopped Timer");

        this.running = true;
        this.cameraTimer.schedule(new TargetTimerTask(), 0);
    }

    public void pause()
    {
        this.running = false;
        this.cameraTimer.cancel();
    }

    public void stop(){
        pause();
        this.cameraTimer = null;
    }


    //---------------------------------------------------------------------
    
    protected void pollImage() throws NIVisionException, AxisCameraException{
        if(this.camera.freshImage()){
            ColorImage color_image = this.camera.getImage();
            MonoImage lum_plane = color_image.getLuminancePlane();

            //TODO MANGO
            this.targets = lum_plane.detectEllipses(this.ellipseDescriptor);
            /*if(this.targets == null){
                System.out.println("Targets is null");
            }else{
                System.out.println("Targets size: " + this.targets.length);
                for(int i = 0; i < targets.length; ++i)
                    System.out.println("Targets num: " + i + " | Center x: " + this.targets[i].m_xPos + "," + this.targets[i].m_yPos);
            }*/

            color_image.free();
            lum_plane.free();
        }else{
            System.out.println("No Fresh Image Avaiable");
        }
    }

    //---------------------------------------------------------------------

    protected void processImage()
    {

        synchronized(LOCK) {

            if(this.targets == null)
                return;

            //partition ellipses based on common centers
            Vector centerMatchLists = centerPartitioner.partitionSharedCenters(
                                                                      targets);

            /*if(centerMatchLists != null){
                System.out.println("Size of Partitioned Vector : " + centerMatchLists.size());
                for(int i = 0; i < centerMatchLists.size(); ++i){
                    Vector currentMatchVector = (Vector)(centerMatchLists.elementAt(i));
                    for(int j = 0; j < currentMatchVector.size(); ++j){
                        EllipseMatch currentMatch = (EllipseMatch)(currentMatchVector.elementAt(j));
                        System.out.println("Target Num : " + i + "/" + j + " | Center: " + currentMatch.m_xPos + ", " + currentMatch.m_yPos);
                        double area = currentMatch.m_majorRadius * currentMatch.m_minorRadius;
                        System.out.println("Target's Area : " + area);
                    }
                }
            }
            else
                System.out.println("Paritioned Vector returned Null");*/

            boolean targetFound = false;

            //if we had some matches, lets compare their relative areas
            if (centerMatchLists != null && !centerMatchLists.isEmpty())
            {
                for (int i = 0; !targetFound && i < centerMatchLists.size(); ++i)
                {
                    Vector cMatchList = (Vector) centerMatchLists.elementAt(i);
                    
                    if (cMatchList.size() < Constants.TARGET_MIN_ELLIPSE_COUNT)
                        continue;

                    boolean areaLooksRight = areaChecker.getEllipsesWithSameAreaRatio(cMatchList);

                    //System.out.println("Does Area Look right: " + areaLooksRight);

                    if (areaLooksRight)
                    {
                        this.center_detected = true;
                        this.center = centerPartitioner.getAverageCenter(cMatchList);
                        this.normalizedCenter = normalize(this.center);
                        targetFound = true;
                    }

                }
            }

            if (!targetFound)
            {
                this.center_detected = true;
                this.center = null;
                this.normalizedCenter = null;
            }

            /*if(targetFound)
                System.out.println("Calculated Ratio Center| X: " + this.center.getX() + " | Y: " + this.center.getY());
            else
                System.out.println("Cannot find any calculated ratio Center");*/
        }
    }

    //---------------------------------------------------------------------
    
    protected void processImageOld(){
        synchronized(LOCK) {
            EllipseMatch matchOne = this.targets[0];
            EllipseMatch matchTwo = this.targets[1];

            //TODO MANGO TEST MORE
            if((Math.abs(matchOne.m_xPos - matchTwo.m_xPos) < CENTER_DELTA) &&
                    (Math.abs(matchOne.m_yPos - matchTwo.m_yPos) < CENTER_DELTA)){
                    this.center_detected = true;
                    this.center = new Point2D((int)((matchOne.m_xPos + matchTwo.m_xPos)/2),
                            (int)((matchOne.m_yPos + matchTwo.m_yPos)/2));
            }else{
                this.center = null;
                this.center_detected = false;
            }
        }
    }

    //---------------------------------------------------------------------

    /**
     * Returns the most recently calculated target center in the image,
     * or null if target not found.
     * @return Target coordinates, in image coord space
     */
    
    public Point2D getCenter(){
        Point2D ctr = null;

        synchronized(LOCK){
            if(this.center_detected)
                ctr = this.center;
        }

        return ctr;
    }

    //---------------------------------------------------------------------

    
    public Point2DDouble getNormalizedCenter(){

        Point2DDouble nCenter = null;

        synchronized(LOCK){
            if(this.center_detected)
            {
                nCenter = this.normalizedCenter;
            }
        }

        return nCenter;
    }


    //---------------------------------------------------------------------

    /**
     * Takes a Point2D in image coordinates and returns the
     * associated normalized point, between -1 and 1 for
     * both dimensions.
     * @param point Input point in image coords
     * @return Point in image range coords [-1, 1]
     */
    
    protected Point2DDouble normalize(Point2D point)
    {
        if (point == null)
            return null;

        double x = point.getX();
        double y = point.getY();

        double w = ((x - this.cameraHalfWidth)  / this.cameraHalfWidth);
        double h = ((y - this.cameraHalfHeight) / this.cameraHalfHeight);

        Point2DDouble normPt = new Point2DDouble(w, h);

        return normPt;
    }

    //---------------------------------------------------------------------

    class TargetTimerTask extends TimerTask {
          public void run()
            {
                try{
                    //collect image from camera
                    pollImage();

                    //processing involves looking for target
                    processImage();

                    if (Targetting.this.running)
                        cameraTimer.schedule(new TargetTimerTask(), delayTime);

                } catch(Exception e){
                    e.printStackTrace();
                }
            }
        };
}



